#include "head_task.h"

uint8_t RUI_F_HEAD_AIM_INIT(void)
{
    if (RUI_ROOT.MOTOR_Pitch_2006 == RUI_DF_OFFLINE ||
        RUI_ROOT.MOTOR_Roll_2006 == RUI_DF_OFFLINE)
    {
        return RUI_DF_ERROR;
    }

    if (HAL_GetTick() < 5000)
    {
        return RUI_DF_ERROR;
    }

    RUI_F_HEAD_MOTOR_CLEAR(&WHW_MOTOR_Pitch , 0);
    RUI_F_HEAD_MOTOR_CLEAR(&WHW_MOTOR_Roll , 0);

    return RUI_DF_READY;
}

uint8_t heading(uint8_t DBUS_STATUS)
{
    static uint8_t AIM_INIT = RUI_DF_ERROR;
    static uint8_t PID_INIT=RUI_DF_ERROR;

    //电机PID赋值
    if (PID_INIT != RUI_DF_READY)
    {
        PID_INIT = WHW_MOTER_Init();
        return RUI_DF_ERROR;
    }
    //电机在线检测
    if (AIM_INIT != RUI_DF_READY)
    {
        AIM_INIT = RUI_F_HEAD_AIM_INIT();
        return RUI_DF_ERROR;
    }
	
#ifdef R_Matrix_d
    //目标赋值
    WHW_MOTOR_Pitch.DATA.Aim = Axial_Err.long_m * Axial_Err.Angle_0;
    WHW_MOTOR_Roll.DATA.Aim = Axial_Err.long_l * Axial_Err.Angle_0;
#endif
	
#ifdef Q_info_d
	//目标赋值
	WHW_MOTOR_Pitch.DATA.Aim = (Q_info_Error_Linear.q2*1000);
	WHW_MOTOR_Roll.DATA.Aim = (Q_info_Error_Linear.q1*1000);	
#endif
	
    //遥控离线保护
    if (!DBUS_STATUS)
    {
        /*限幅归零*/

        WHW_MOTOR_Pitch.PID_P.max_iout = 0;
        WHW_MOTOR_Pitch.PID_S.max_iout = 0;

        WHW_MOTOR_Roll.PID_P.max_iout = 0;
        WHW_MOTOR_Roll.PID_S.max_iout = 0;

        /*误差值归零*/

        WHW_MOTOR_Pitch.DATA.Aim = (float)WHW_MOTOR_Pitch.DATA.Angle_Infinite;
        WHW_MOTOR_Pitch.DATA.Aim = (float)WHW_MOTOR_Pitch.DATA.Angle_Infinite;

        WHW_MOTOR_Roll.DATA.Aim = (float)WHW_MOTOR_Roll.DATA.Angle_Infinite;
        WHW_MOTOR_Roll.DATA.Aim = (float)WHW_MOTOR_Roll.DATA.Angle_Infinite;
    }

    //电机堵转检测
    RUI_F_HEAD_MOTOR3508_STUCK(&WHW_MOTOR_Pitch ,
                               8000 ,
                               10);
    RUI_F_HEAD_MOTOR3508_STUCK(&WHW_MOTOR_Roll ,
                               8000 ,
                               10);

    //电机前馈计算
    Feedforward_Calculate(&WHW_MOTOR_Pitch.PID_F ,
                          WHW_MOTOR_Pitch.DATA.Aim);
    Feedforward_Calculate(&WHW_MOTOR_Roll.PID_F ,
                          WHW_MOTOR_Roll.DATA.Aim);

    //电机角度计算
    PID_calc(&WHW_MOTOR_Pitch.PID_P ,
             0 ,
             WHW_MOTOR_Pitch.DATA.Aim);
    PID_calc(&WHW_MOTOR_Roll.PID_P ,
             0 ,
             WHW_MOTOR_Roll.DATA.Aim);

    //电机速度计算
    PID_calc(&WHW_MOTOR_Pitch.PID_S ,
             WHW_MOTOR_Pitch.DATA.Speed_now ,
             WHW_MOTOR_Pitch.PID_P.out);
    PID_calc(&WHW_MOTOR_Roll.PID_S ,
             WHW_MOTOR_Roll.DATA.Speed_now ,
             WHW_MOTOR_Roll.PID_P.out);

	return 0;
}
